
#ifndef MOT_PRIV_H
#define MOT_PRIV_H
/***********************************************************************
*                       TYPEDEFS, ENUMS, ETC.                          *
************************************************************************/

/* First we define structures for data shared with the HAL */

/* HAL visible data notations:
   RPA:  read only parameter
   WPA:  write only parameter
   WRPA: read/write parameter
   RPI:  read only pin
   WPI:  write only pin
   WRPI: read/write pin
*/

/* joint data */
#include "hal/hal.h"
#include "zuc/motion/motion.h"
#include "log/zuclog.h"
#include "config.h"
#include "zuc/motion/mot_halStruct.h"
#include "modbus/mb_slave.h"

#define PAYLOAD_IDENTIFY_VAL1 5.0
#define PAYLOAD_IDENTIFY_VAL2 2.0
#define MAX_JOG_JOINT_ACC 90.0
#define MAX_JOG_JOINT_ACC_COBOT 70.0
#define MAX_JOG_AXIS_ACC_TRANS 500.0
#define MAX_JOG_AXIS_ACC_ROT 90.0

#ifdef __cplusplus
extern "C" {
#endif
/***********************************************************************
*                   GLOBAL VARIABLE DECLARATIONS                       *
************************************************************************/

/* pointer to zucmot_hal_data_t struct in HAL shmem, with all HAL data */
extern zucmot_hal_data_t* zucmot_hal_data;

extern zucmot_motor_t* motors;

/* pointer to array of joint structs with all joint data */
/* the actual array may be in shared memory or in kernel space, as
   determined by the init code in motion.c */
extern zucmot_joint_t* joints;

/* pointer to array of axis structs with all axis data */
extern zucmot_axis_t* axes;

/* these variable have the 1/servo cycle time */
extern double servo_freq;

/* Struct pointers */
extern struct zucmot_struct_t* zucmotStruct;
extern struct zucmot_command_t* zucmotCommand;
extern struct zucmot_status_t* zucmotStatus;
extern struct zucmot_config_t* zucmotConfig;
extern struct zucmot_debug_t* zucmotDebug;
extern struct zucmot_error_t* zucmotError;

extern ModbusIOShm_t* zucmotExtIOStruct;
extern PhysicIOShm_t* zucmotPhysicIOStruct;
extern struct PNDevShm* zucmotPNDevStruct;
extern struct EIPShm* zucmotEIPStruct;
extern struct SensorShm* zucmotSensorStruct;
extern struct TorqueControlShm* zucmotTorqueControlStruct;

/***********************************************************************
*                    PUBLIC FUNCTION PROTOTYPES                        *
************************************************************************/
/**单步调试状态标记共享*/
extern void usrmotSetSteppingState(int val);
extern int usrmotGetSteppingState();

/* function definitions */
extern void zucmotCommandHandler(void* arg);
extern void zucmotController(void* arg);
extern void scb_comm_init(int scb_cab_type);
extern void scb_can_comm(void* arg);
// extern void scb_stop_comm();
extern void zucmotSetCycleTime(unsigned long nsec);

/* these are related to synchronized I/O */

extern ZucPose getFinalUserOffset();
extern ZucPose tracking_get_revisedpos();
extern void get_vel_theta(double* vel_for_convyr, double* theta);
extern void get_tar_pos(const double left_theta, ZucPose* tar_pos);
int track_pre_singular();  //传送带跟踪过程中预判到奇异的时候做相关处理
int max_distance_stop();
extern void set_dis_convytracking_cfg();
//
// Try to change the Motion mode to Teleop.
//
// This function can be called at any time.  Returns without changing
// mode if Teleop is not currently allowed.  This code doesn't actually
// make the transition, it just sets a flag requesting the transition.
// The real transition to Teleop mode is done in zucmotController().
//
void switch_to_teleop_mode(void);

void switch_to_teleop_tool_mode(void);

void switch_to_servojop_mode();

void switch_to_drag_mode(void);

void switch_to_admittance_mode(void);

void setDragModeBit(bool bStat);

/* loops through the active joints and checks if any are not homed */
extern int checkAllHomed(void);

/* handles 'homed' flags, see command.c for details */
extern void clearHomes(int joint_num);

extern void zucmot_config_change(void);

// extern void servo_update_payload();

extern void reportError(int errcode, const char* fmt, ...) __attribute((format(printf, 2, 3))); /* Use the rtapi_print call */

extern void reportWarning(int errcode, const char* fmt, ...) __attribute((format(printf, 2, 3))); /* Use the rtapi_print call */

extern void reportInfo(int infocode, const char* fmt, ...) __attribute((format(printf, 2, 3))); /* Use the rtapi_print call */

/* rtapi_get_time() returns a nanosecond value. In time, we should use a u64
    value for all calcs and only do the conversion to seconds when it is
    really needed. */
#define etime() (((double)rtapi_get_time()) / 1.0e9)

/* macros for reading, writing bit flags */

/* motion flags */

#define GET_MOTION_ERROR_FLAG() (zucmotStatus->motionFlag & ZUCMOT_MOTION_ERROR_BIT ? 1 : 0)

#define SET_MOTION_ERROR_FLAG(fl)                            \
    if (fl)                                                  \
        zucmotStatus->motionFlag |= ZUCMOT_MOTION_ERROR_BIT; \
    else                                                     \
        zucmotStatus->motionFlag &= ~ZUCMOT_MOTION_ERROR_BIT;

#define GET_MOTION_COORD_FLAG() (zucmotStatus->motionFlag & ZUCMOT_MOTION_COORD_BIT ? 1 : 0)

#define SET_MOTION_COORD_FLAG(fl)                            \
    if (fl)                                                  \
        zucmotStatus->motionFlag |= ZUCMOT_MOTION_COORD_BIT; \
    else                                                     \
        zucmotStatus->motionFlag &= ~ZUCMOT_MOTION_COORD_BIT;

#define GET_MOTION_TELEOP_FLAG() (zucmotStatus->motionFlag & ZUCMOT_MOTION_TELEOP_BIT ? 1 : 0)

#define SET_MOTION_TELEOP_FLAG(fl)                            \
    if (fl)                                                   \
        zucmotStatus->motionFlag |= ZUCMOT_MOTION_TELEOP_BIT; \
    else                                                      \
        zucmotStatus->motionFlag &= ~ZUCMOT_MOTION_TELEOP_BIT;

#define GET_MOTION_TELEOP_TOOL_FLAG() (zucmotStatus->motionFlag & ZUCMOT_MOTION_TELEOP_TOOL_BIT ? 1 : 0)

#define SET_MOTION_TELEOP_TOOL_FLAG(fl)                            \
    if (fl)                                                        \
        zucmotStatus->motionFlag |= ZUCMOT_MOTION_TELEOP_TOOL_BIT; \
    else                                                           \
        zucmotStatus->motionFlag &= ~ZUCMOT_MOTION_TELEOP_TOOL_BIT;

#define GET_MOTION_SERVOMOVEOP_FLAG() (zucmotStatus->motionFlag & ZUCMOT_MOTION_SERVOJOP_BIT ? 1 : 0)

#define SET_MOTION_SERVOMOVEOP_FLAG(fl)                         \
    if (fl)                                                     \
        zucmotStatus->motionFlag |= ZUCMOT_MOTION_SERVOJOP_BIT; \
    else                                                        \
        zucmotStatus->motionFlag &= ~ZUCMOT_MOTION_SERVOJOP_BIT;

#define GET_MOTION_ADMITTANCE_FLAG() (zucmotStatus->motionFlag & ZUCMOT_MOTION_ADMITTANCE_BIT ? 1 : 0)

#define SET_MOTION_ADMITTANCE_FLAG(fl)                            \
    if (fl)                                                       \
        zucmotStatus->motionFlag |= ZUCMOT_MOTION_ADMITTANCE_BIT; \
    else                                                          \
        zucmotStatus->motionFlag &= ~ZUCMOT_MOTION_ADMITTANCE_BIT;

#define GET_MOTION_DRAG_FLAG() (zucmotStatus->motionFlag & ZUCMOT_MOTION_DRAG_BIT ? 1 : 0)

#define SET_MOTION_DRAG_FLAG(fl)                            \
    if (fl)                                                 \
        zucmotStatus->motionFlag |= ZUCMOT_MOTION_DRAG_BIT; \
    else                                                    \
        zucmotStatus->motionFlag &= ~ZUCMOT_MOTION_DRAG_BIT;

#define GET_MOTION_INPOS_FLAG() (zucmotStatus->motionFlag & ZUCMOT_MOTION_INPOS_BIT ? 1 : 0)

#define SET_MOTION_INPOS_FLAG(fl)                            \
    if (fl)                                                  \
        zucmotStatus->motionFlag |= ZUCMOT_MOTION_INPOS_BIT; \
    else                                                     \
        zucmotStatus->motionFlag &= ~ZUCMOT_MOTION_INPOS_BIT;

#define GET_MOTION_ENABLE_FLAG() (zucmotStatus->motionFlag & ZUCMOT_MOTION_ENABLE_BIT ? 1 : 0)

#define SET_MOTION_ENABLE_FLAG(fl)                            \
    if (fl)                                                   \
        zucmotStatus->motionFlag |= ZUCMOT_MOTION_ENABLE_BIT; \
    else                                                      \
        zucmotStatus->motionFlag &= ~ZUCMOT_MOTION_ENABLE_BIT;

#define SET_MOTION_HOME_FLAG(fl)                            \
    if (fl)                                                 \
        zucmotStatus->motionFlag |= ZUCMOT_MOTION_HOME_BIT; \
    else                                                    \
        zucmotStatus->motionFlag &= ~ZUCMOT_MOTION_HOME_BIT;

#define GET_MOTION_HOME_FLAG() (zucmotStatus->motionFlag & ZUCMOT_MOTION_HOME_BIT ? 1 : 0)

/* joint flags */

#define GET_JOINT_ENABLE_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_ENABLE_BIT ? 1 : 0)

#define SET_JOINT_ENABLE_FLAG(joint, fl)          \
    if (fl)                                       \
        (joint)->flag |= ZUCMOT_JOINT_ENABLE_BIT; \
    else                                          \
        (joint)->flag &= ~ZUCMOT_JOINT_ENABLE_BIT;

#define GET_JOINT_ACTIVE_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_ACTIVE_BIT ? 1 : 0)

#define SET_JOINT_ACTIVE_FLAG(joint, fl)          \
    if (fl)                                       \
        (joint)->flag |= ZUCMOT_JOINT_ACTIVE_BIT; \
    else                                          \
        (joint)->flag &= ~ZUCMOT_JOINT_ACTIVE_BIT;

#define GET_JOINT_INPOS_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_INPOS_BIT ? 1 : 0)

#define SET_JOINT_INPOS_FLAG(joint, fl)          \
    if (fl)                                      \
        (joint)->flag |= ZUCMOT_JOINT_INPOS_BIT; \
    else                                         \
        (joint)->flag &= ~ZUCMOT_JOINT_INPOS_BIT;

#define GET_JOINT_ERROR_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_ERROR_BIT ? 1 : 0)

#define SET_JOINT_ERROR_FLAG(joint, fl)          \
    if (fl)                                      \
        (joint)->flag |= ZUCMOT_JOINT_ERROR_BIT; \
    else                                         \
        (joint)->flag &= ~ZUCMOT_JOINT_ERROR_BIT;

#define GET_JOINT_PHL_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_MAX_HARD_LIMIT_BIT ? 1 : 0)

#define SET_JOINT_PHL_FLAG(joint, fl)                     \
    if (fl)                                               \
        (joint)->flag |= ZUCMOT_JOINT_MAX_HARD_LIMIT_BIT; \
    else                                                  \
        (joint)->flag &= ~ZUCMOT_JOINT_MAX_HARD_LIMIT_BIT;

#define GET_JOINT_NHL_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_MIN_HARD_LIMIT_BIT ? 1 : 0)

#define SET_JOINT_NHL_FLAG(joint, fl)                     \
    if (fl)                                               \
        (joint)->flag |= ZUCMOT_JOINT_MIN_HARD_LIMIT_BIT; \
    else                                                  \
        (joint)->flag &= ~ZUCMOT_JOINT_MIN_HARD_LIMIT_BIT;

#define GET_JOINT_HOME_SWITCH_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_HOME_SWITCH_BIT ? 1 : 0)

#define SET_JOINT_HOME_SWITCH_FLAG(joint, fl)          \
    if (fl)                                            \
        (joint)->flag |= ZUCMOT_JOINT_HOME_SWITCH_BIT; \
    else                                               \
        (joint)->flag &= ~ZUCMOT_JOINT_HOME_SWITCH_BIT;

#define GET_JOINT_HOMING_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_HOMING_BIT ? 1 : 0)

#define SET_JOINT_HOMING_FLAG(joint, fl)          \
    if (fl)                                       \
        (joint)->flag |= ZUCMOT_JOINT_HOMING_BIT; \
    else                                          \
        (joint)->flag &= ~ZUCMOT_JOINT_HOMING_BIT;

#define GET_JOINT_HOMED_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_HOMED_BIT ? 1 : 0)

#define SET_JOINT_HOMED_FLAG(joint, fl)          \
    if (fl)                                      \
        (joint)->flag |= ZUCMOT_JOINT_HOMED_BIT; \
    else                                         \
        (joint)->flag &= ~ZUCMOT_JOINT_HOMED_BIT;

#define GET_JOINT_AT_HOME_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_AT_HOME_BIT ? 1 : 0)

#define SET_JOINT_AT_HOME_FLAG(joint, fl)          \
    if (fl)                                        \
        (joint)->flag |= ZUCMOT_JOINT_AT_HOME_BIT; \
    else                                           \
        (joint)->flag &= ~ZUCMOT_JOINT_AT_HOME_BIT;

#define GET_JOINT_FERROR_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_FERROR_BIT ? 1 : 0)

#define SET_JOINT_FERROR_FLAG(joint, fl)          \
    if (fl)                                       \
        (joint)->flag |= ZUCMOT_JOINT_FERROR_BIT; \
    else                                          \
        (joint)->flag &= ~ZUCMOT_JOINT_FERROR_BIT;

#define GET_JOINT_FAULT_FLAG(joint) ((joint)->flag & ZUCMOT_JOINT_FAULT_BIT ? 1 : 0)

#define SET_JOINT_FAULT_FLAG(joint, fl)          \
    if (fl)                                      \
        (joint)->flag |= ZUCMOT_JOINT_FAULT_BIT; \
    else                                         \
        (joint)->flag &= ~ZUCMOT_JOINT_FAULT_BIT;

#define ONE_BASED(joint_0_based) (joint_0_based + 1)

#if defined(__KERNEL__)
#define HAVE_CPU_KHZ
#endif
#ifdef __cplusplus
}
#endif
#endif /* MOT_PRIV_H */
